#!/usr/bin/env python
# -*- coding: utf-8 -*-
import rospy
import action_point as ap
import local_pyhop as ph
import methods_point2
import sys, termios,time



def __init__():
    rospy.init_node('Task_aera', anonymous=True)
    print('Vehicle initialized complete!')

if __name__=="__main__":
    __init__()
    rate = rospy.Rate(10)
    task,area,target = ap.get_infor()
    state = ap.get_state()
    result = ph.pyhop(state, [(task, area, target)], verbose=2)
    print('Task breakdown completed！Vehicle start to execute the task!')
    time.sleep(2)
    for i in range(len(result)):
        #给函数传入多个参数，用字符串来调用函数
        if result[i-1][0] == 'scan_high' and ap.multi_aruco_info.num_arucos == 0:
            continue
        parameters = result[i][1:]
        getattr(ap, result[i][0])(rate, *parameters)
        if result[i][0] == 'scan_close' and ap.aruco_info.detected == True :
            break
    if ap.aruco_info.detected == True:
        for j in range(len(result)-2,len(result)):
            parameters = result[j][1:]
            getattr(ap, result[j][0])(rate, *parameters)
        print("Execute task over!")
    else:
        print("No target found in the area!\nExecute task over!\n")
